package math2;

import fem2.jlinalg.EigenvalueDecomposition;
import iceb.jnative.lapack.JLapack;
import inf.jlinalg.Array1DMatrix;
import inf.jlinalg.Array2DMatrix;
import inf.jlinalg.BLAM;
import inf.jlinalg.IMatrix;
import inf.jlinalg.IMatrixReadOnly;
import inf.jlinalg.IVector;
import inf.jlinalg.JArpackSBMGESolver;
import inf.jlinalg.SolveFailedException;


import org.omg.CORBA.IntHolder;

/**
 * A set of utilities used for linear algebra
 * 
 * @author hbui
 * 
 */
public class LinAlgUtilities {

	public static Array2DMatrix OneByOne2d = new Array2DMatrix(new double[][] { //
			{ 1.0, 1.0, 0.0 }, //
					{ 1.0, 1.0, 0.0 }, //
					{ 0.0, 0.0, 0.0 } //
			});

	public static Array2DMatrix OneByOne3d = new Array2DMatrix(new double[][] {
			{ 1.0, 1.0, 1.0, 0.0, 0.0, 0.0 }, //
			{ 1.0, 1.0, 1.0, 0.0, 0.0, 0.0 }, //
			{ 1.0, 1.0, 1.0, 0.0, 0.0, 0.0 }, //
			{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, //
			{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, //
			{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } //
			});

	public static Array2DMatrix Rank4Identity2d = new Array2DMatrix(new double[][] { //
			{ 1.0, 0.0, 0.0 }, //
					{ 0.0, 1.0, 0.0 }, //
					{ 0.0, 0.0, 0.5 } //
			});

	public static Array2DMatrix Rank4Identity3d = new Array2DMatrix(new double[][] { //
			{ 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, //
					{ 0.0, 1.0, 0.0, 0.0, 0.0, 0.0 }, //
					{ 0.0, 0.0, 1.0, 0.0, 0.0, 0.0 }, //
					{ 0.0, 0.0, 0.0, 0.5, 0.0, 0.0 }, //
					{ 0.0, 0.0, 0.0, 0.0, 0.5, 0.0 }, //
					{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.5 } //
			});

	// public static IMatrix Idev2d = new Array2DMatrix(new double[][] { //
	// { 2.0 / 3, -1.0 / 3, 0.0 }, //
	// { -1.0 / 3, 2.0 / 3, 0.0 }, //
	// { 0.0, 0.0, 1.0 } //
	// });
	public static Array2DMatrix Idev2d = new Array2DMatrix(new double[][] { //
			{ 2.0 / 3, -1.0 / 3, 0.0 }, //
					{ -1.0 / 3, 2.0 / 3, 0.0 }, //
					{ 0.0, 0.0, 0.5 } //
			});

	// public static IMatrix Idev3d = new Array2DMatrix(new double[][] { //
	// { 2.0 / 3, -1.0 / 3, -1.0 / 3, 0.0, 0.0, 0.0 }, //
	// { -1.0 / 3, 2.0 / 3, -1.0 / 3, 0.0, 0.0, 0.0 }, //
	// { -1.0 / 3, -1.0 / 3, 2.0 / 3, 0.0, 0.0, 0.0 }, //
	// { 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 }, //
	// { 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 }, //
	// { 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 } //
	// });
	public static Array2DMatrix Idev3d = new Array2DMatrix(new double[][] { //
			{ 2.0 / 3, -1.0 / 3, -1.0 / 3, 0.0, 0.0, 0.0 }, //
					{ -1.0 / 3, 2.0 / 3, -1.0 / 3, 0.0, 0.0, 0.0 }, //
					{ -1.0 / 3, -1.0 / 3, 2.0 / 3, 0.0, 0.0, 0.0 }, //
					{ 0.0, 0.0, 0.0, 0.5, 0.0, 0.0 }, //
					{ 0.0, 0.0, 0.0, 0.0, 0.5, 0.0 }, //
					{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.5 } //
			});

	public final static double[][] FOID = new double[][] { //
	{ 1.0, 0.0, 0.0, 0.0 },//
			{ 0.0, 1.0, 0.0, 0.0 }, //
			{ 0.0, 0.0, 0.5, 0.0 }, //
			{ 0.0, 0.0, 0.0, 1.0 } };
	public final static double[] SOID = new double[] { 1.0, 1.0, 0.0, 1.0 };
	public final static double[] SOID3d = new double[] { 1.0, 1.0, 1.0, 0.0, 0.0, 0.0 };

	private static EigenvalueDecomposition PrincipalStressStrainDecomposer = new EigenvalueDecomposition(
			true);

	/**
	 * 
	 * @param A
	 *            = [a11 a12 a21 a22]
	 * @return the determinant of A
	 */
	public static double determinant2x2(double... A) {
		if (A.length != 4) {
			throw new Error("not a 2x2 matrix");
		}
		return A[0] * A[3] - A[1] * A[2];
	}

	/**
	 * 
	 * @param A
	 *            = [a11 a12; a21 a22]
	 * @return the determinant of A
	 */
	public static double determinant2x2(double[][] A) {
		if (A.length != 2) {
			throw new Error("not a 2x2 matrix");
		}
		return A[0][0] * A[1][1] - A[0][1] * A[1][0];
	}

	/**
	 * 
	 * @param A
	 *            = [a11 a12 a13 a21 a22 a23 a31 a32 a33]
	 * @return the determinant of A
	 */
	public static double determinant3x3(double... A) {
		if (A.length != 9) {
			throw new Error("not a 3x3 matrix");
		}
		return A[0] * (A[4] * A[8] - A[5] * A[7]) - A[1] * (A[3] * A[8] - A[5] * A[6]) + A[2]
				* (A[3] * A[7] - A[4] * A[6]);
	}

	/**
	 * @param N
	 * @param A
	 *            square matrix N by N
	 * @return the determinant of A
	 */
	public static double determinantNxN(int N, double... A) {
		if (N == 2) {
			return determinant2x2(A);
		} else if (N == 3) {
			return determinant3x3(A);
		}
		throw new Error("not supported matrix");
	}

	/**
	 * 
	 * @param A
	 *            = [a11 a12 a13; a21 a22 a23; a31 a32 a33] = [a00 a01 a02; a10
	 *            a11 a12; a20 a21 a22] 0 1 2 3 4 5 6 7 8
	 * @return the determinant of A
	 */
	public static double determinant3x3(double[][] A) {
		if (A.length != 3) {
			throw new Error("not a 3x3 matrix");
		}
		// return A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) - A[0][1]
		// * (A[1][0] * A[2][2] - A[1][2] * A[2][0]) + A[0][2]
		// * (A[1][0] * A[2][1] - A[1][1] * A[2][0]);
		return -A[0][2] * A[1][1] * A[2][0] + A[0][1] * A[1][2] * A[2][0] + A[0][2] * A[1][0]
				* A[2][1] - A[0][0] * A[1][2] * A[2][1] - A[0][1] * A[1][0] * A[2][2] + A[0][0]
				* A[1][1] * A[2][2];
	}

	/**
	 * @param A
	 *            square matrix N by N
	 * @return the determinant of A
	 */
	public static double determinantNxN(double[][] A) {
		if (A.length == 2) {
			return determinant2x2(A);
		} else if (A.length == 3) {
			return determinant3x3(A);
		} else if (A.length == 1) {
			return A[0][0];
		}
		throw new Error("not supported matrix");
	}

	/**
	 * 
	 * @param J
	 * @return det(J * J^T)
	 */
	public static double determinantJJt(double[][] J) {
		int m = J.length;
		int n = J[0].length;
		double[][] JJt = new double[m][m];
		for (int i = 0; i < m; i++) {
			for (int j = 0; j < m; j++) {
				for (int k = 0; k < n; k++) {
					JJt[i][j] += J[i][k] * J[j][k];
				}
			}
		}
		return determinantNxN(JJt);
	}

	/**
	 * invert matrix A
	 * 
	 * @param A
	 *            = [a11 a12 a21 a22]
	 */
	public static void invert2x2(double... A) {
		if (A.length != 4) {
			throw new Error("not a 2x2 matrix");
		}
		double det = determinant2x2(A);
		if (Math.abs(det) < MathUtilities.tol14) {
			throw new IllegalArgumentException("Singular matrix");
		}
		A[0] /= det;
		A[2] /= -det;
		A[1] /= -det;
		A[3] /= det;
		double tmp = A[0];
		A[0] = A[3];
		A[3] = tmp;
	}

	/**
	 * invert matrix A
	 * 
	 * @param A
	 *            = [a11 a12][ a21 a22]
	 */
	public static void invert2x2(double[][] A) {
		if (A.length != 2) {
			throw new Error("not a 2x2 matrix");
		}
		double det = determinant2x2(A);
		if (Math.abs(det) < MathUtilities.tol14) {
			throw new IllegalArgumentException("Singular matrix");
		}
		A[0][0] /= det;
		A[1][0] /= -det;
		A[0][1] /= -det;
		A[1][1] /= det;
		double tmp = A[0][0];
		A[0][0] = A[1][1];
		A[1][1] = tmp;
	}

	/**
	 * 
	 * @param A
	 *            = [a11 a12 a13 a21 a22 a23 a31 a32 a33]
	 * @return the determinant of A
	 */
	public static void invert3x3(double... A) {
		/*
		 * TODO
		 */
		throw new Error("not implemented");
	}

	/**
	 * 
	 * @param A
	 *            = [a11 a12 a13; a21 a22 a23; a31 a32 a33]
	 * @return the determinant of A
	 */
	public static void invert3x3(double[][] A) {
		if (A.length != 3) {
			throw new Error("not a 3x3 matrix");
		}
		double det = determinant3x3(A);
		double[] B = new double[9];
		B[0] = -A[1][2] * A[2][1] + A[1][1] * A[2][2];
		B[1] = A[0][2] * A[2][1] - A[0][1] * A[2][2];
		B[2] = -A[0][2] * A[1][1] + A[0][1] * A[1][2];
		B[3] = A[1][2] * A[2][0] - A[1][0] * A[2][2];
		B[4] = -A[0][2] * A[2][0] + A[0][0] * A[2][2];
		B[5] = A[0][2] * A[1][0] - A[0][0] * A[1][2];
		B[6] = -A[1][1] * A[2][0] + A[1][0] * A[2][1];
		B[7] = A[0][1] * A[2][0] - A[0][0] * A[2][1];
		B[8] = -A[0][1] * A[1][0] + A[0][0] * A[1][1];
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 3; j++) {
				A[i][j] = B[i * 3 + j] / det;
			}
		}
	}

	public static void invertNxN(double[][] A) {
		if (A.length == 2) {
			invert2x2(A);
		} else if (A.length == 3) {
			invert3x3(A);
		} else {
			throw new Error("not supported matrix");
		}
	}

	/**
	 * Computes y <- alpha * x + y without checking dimension. User are
	 * responsible for ensuring dimension of x is greater than or equal
	 * dimension of y
	 * 
	 * @param alpha
	 *            scaling factor for x
	 * @param x
	 *            the vector x
	 * @param y
	 *            the vector y
	 */
	public static void add(double alpha, IVector x, IVector y) {
		for (int i = 0; i < y.getSize(); i++) {
			y.set(i, alpha * x.get(i) + y.get(i));
		}
	}

	/**
	 * Computes the dyadic product A &lt;- alpha x y^T + A. Perform naive sum to
	 * A without checking dimension of x and y. User are responsible to ensure
	 * dimension of dyadic product greater than or equal dimension of A
	 * 
	 * @param alpha
	 *            scaling factor
	 * @param x
	 *            vector of size m
	 * @param y
	 *            vector of size n
	 * @param a
	 *            matrix A of size m x n
	 */
	public static void multiply(double alpha, IVector x, IVector y, IMatrix a) {
		int m = a.getRowCount();
		int n = a.getColumnCount();

		for (int i = 0; i < m; i++) {
			for (int j = 0; j < n; j++) {
				a.add(i, j, alpha * x.get(i) * y.get(j));
			}
		}
	}

	/**
	 * Matrix vector product: Computes c <- alpha * op(A) * b + beta * c where A
	 * is a matrix and b,c are vectors. The operator op(A) is either A or A'.
	 * 
	 * @param transposeA
	 *            transpose matrix a.
	 * @param m
	 *            row/column count of matrix a.
	 * @param n
	 *            column/row count of matrix a, size of vectors b, c.
	 * @param alpha
	 *            scaling factor.
	 * @param a
	 *            matrix of size m x n or n x n in row major format
	 * @param b
	 *            vector of length n.
	 * @param c
	 *            vector of length m.
	 */
	public static void multiply(boolean transposeA, int m, int n, double alpha, double[] a,
			double[] b, double beta, double[] c) {
		double s;

		if (transposeA) {
			for (int i = 0; i < m; i++) {
				s = 0;
				for (int j = 0; j < n; j++) {
					s += a[j * n + i] * b[j];
				}
				c[i] = alpha * s + beta * c[i];
			}
		} else {
			for (int i = 0; i < m; i++) {
				s = 0;
				for (int j = 0; j < n; j++) {
					s += a[i * n + j] * b[j];
				}
				c[i] = alpha * s + beta * c[i];
			}
		}
	}

	public static void scale(int n, double alpha, double[] v) {
		for (int i = 0; i < n; i++) {
			v[i] *= alpha;
		}
	}

	/**
	 * solve Ax = b or A^Tx = b using simple Gauss elimination
	 * algorithm<blockquote>Please note that, after backSolve, matrix A will be
	 * modified as well. So use it with much care</blockquote>
	 * 
	 * @param trans
	 *            transpose indicator
	 * @param A
	 *            matrix in row major order
	 * @param b
	 */
	public static void backSolve(int trans, double[] A, double[] b) {
		int n = b.length;
		int ifail;
		int[] ipiv_ = new int[n];

		IntHolder fail = new IntHolder(1);

		/*
		 * factorize
		 */
		JLapack.DGETRF(n, n, A, n, ipiv_, fail);
		ifail = fail.value;

		if (ifail < 0) {
			throw new Error("Wrong value in parameter " + (-ifail));
		} else if (ifail > 0) {
			throw new Error("Factorization failed, upper triangle matrix U[" + ifail + "][" + ifail
					+ "] is zero");
		}

		/*
		 * back solve
		 */
		if (trans == BLAM.NO_TRANSPOSE) {
			JLapack.DGETRS("T", n, 1, A, n, ipiv_, b, n, fail);
		} else if (trans == BLAM.TRANSPOSE) {
			JLapack.DGETRS("N", n, 1, A, n, ipiv_, b, n, fail);
		}
		ifail = fail.value;

		if (ifail != 0) {
			throw new Error("Wrong value in parameter " + (-ifail));
		}
	}

	/**
	 * solve Ax = B using simple Gauss elimination algorithm<blockquote>Please
	 * note that, after backSolve, matrix A will be modified as well. So use it
	 * with much care</blockquote>
	 * 
	 * @param A
	 * @param B
	 *            Matrix of right hand side. Columns as right hand side will be
	 *            solved and put back to B
	 */
	public static void backSolve(int trans, double[] A, IMatrix B) {
		/*
		 * factorize A
		 */
		int m = B.getRowCount();
		int ifail;
		int[] ipiv_ = new int[m];

		IntHolder fail = new IntHolder(1);

		JLapack.DGETRF(m, m, A, m, ipiv_, fail);
		ifail = fail.value;

		if (ifail < 0) {
			throw new Error("Wrong value in parameter " + (-ifail));
		} else if (ifail > 0) {
			throw new Error("Factorization failed, upper triangle matrix U[" + ifail + "][" + ifail
					+ "] is zero");
		}
		/*
		 * extract each column of B and solve
		 */
		int n = B.getColumnCount();
		double[] b = new double[m];
		for (int i = 0; i < n; i++) {
			/*
			 * get the column of B
			 */
			for (int j = 0; j < m; j++) {
				b[j] = B.get(j, i);
			}
			/*
			 * solve
			 */
			if (trans == BLAM.NO_TRANSPOSE) {
				JLapack.DGETRS("T", m, 1, A, m, ipiv_, b, m, fail);
			} else if (trans == BLAM.TRANSPOSE) {
				JLapack.DGETRS("N", m, 1, A, m, ipiv_, b, m, fail);
			}
			ifail = fail.value;

			if (ifail != 0) {
				throw new Error("Error ar column" + i + ": Wrong value in parameter " + (-ifail));
			}
			/*
			 * pass back solved values to B's column
			 */
			for (int j = 0; j < m; j++) {
				B.set(j, i, b[j]);
			}
		}
	}

	/**
	 * solve Ax = b using simple Gauss elimination algorithm
	 * 
	 * @param A
	 * @param b
	 */
	public static void backSolve(Array1DMatrix A, double[] b) {
		if (A.getOrdering() == Array1DMatrix.ORDERING_ROW_MAJOR) {
			backSolve(BLAM.NO_TRANSPOSE, A.getValues(), b);
		} else if (A.getOrdering() == Array1DMatrix.ORDERING_COLUMN_MAJOR) {
			backSolve(BLAM.TRANSPOSE, A.getValues(), b);
		} else {
			throw new Error("invalid matrix A");
		}
	}

	/**
	 * solve Ax = b using simple Gauss elimination algorithm<blockquote>Please
	 * note that, after backSolve, matrix A will be modified as well. So use it
	 * with much care</blockquote>
	 * 
	 * @param A
	 * @param b
	 */
	public static void backSolve(IMatrix A, double[] b) {
		int n = A.getRowCount();
		assert n == A.getColumnCount();
		double[] a = new double[n * n];
		for (int i = 0; i < n; i++) {
			for (int j = 0; j < n; j++) {
				a[i * n + j] = A.get(i, j);
			}
		}
		backSolve(BLAM.NO_TRANSPOSE, a, b);
	}

	/**
	 * solve Ax = B using simple Gauss elimination algorithm. <blockquote>Please
	 * note that, after backSolve, matrix A will be modified as well. So use it
	 * with much care</blockquote>
	 * 
	 * @param A
	 * @param B
	 *            Matrix of right hand side. Columns as right hand side will be
	 *            solved an put back to B
	 */
	public static void backSolve(Array1DMatrix A, IMatrix B) {
		if (A.getOrdering() == Array1DMatrix.ORDERING_ROW_MAJOR) {
			backSolve(BLAM.NO_TRANSPOSE, A.getValues(), B);
		} else if (A.getOrdering() == Array1DMatrix.ORDERING_COLUMN_MAJOR) {
			backSolve(BLAM.TRANSPOSE, A.getValues(), B);
		} else {
			throw new Error("invalid matrix A");
		}
	}

	/**
	 * Computes k <- k + alpha b^T c b.
	 * 
	 * @param b
	 *            matrix of size m x n
	 * @param c
	 *            matrix of size m x m
	 * @param alpha
	 *            scaling factor
	 * @param k
	 *            matrix of size n x n
	 */
	public static void btCB(IMatrixReadOnly b, IMatrixReadOnly c, double alpha, IMatrix k) {
		int m = b.getRowCount();
		int n = b.getColumnCount();
		assert m == c.getRowCount() && m == c.getColumnCount();
		assert n == k.getRowCount() && n == k.getColumnCount();

		// rows of b^T
		for (int i = 0; i < n; i++) {
			// columns of c
			for (int j = 0; j < m; j++) {
				// s is the ij-th entry of b^T c and computes as the
				// dot product of i-th row of b^T with j-th column of c
				double tij = 0;
				for (int ii = 0; ii < m; ii++) {
					tij += b.get(ii, i) * c.get(ii, j);
				}
				// contribution
				for (int jj = 0; jj < n; jj++) {
					k.add(i, jj, alpha * tij * b.get(j, jj));
				}
			}
		}
	}

	/**
	 * Computes k <- k + alpha b c b^T.
	 * 
	 * @param b
	 *            matrix of size m x n
	 * @param c
	 *            matrix of size n x n
	 * @param alpha
	 *            scaling factor
	 * @param k
	 *            matrix of size m x m
	 */
	public static void bCBt(IMatrixReadOnly b, IMatrixReadOnly c, double alpha, IMatrix k) {
		int m = b.getRowCount();
		int n = b.getColumnCount();
		assert n == c.getRowCount() && n == c.getColumnCount();
		assert m == k.getRowCount() && m == k.getColumnCount();

		// rows of b
		for (int i = 0; i < m; i++) {
			// columns of c
			for (int j = 0; j < n; j++) {
				// s is the ij-th entry of b c and computes as the
				// dot product of i-th row of b with j-th column of c
				double tij = 0;
				for (int ii = 0; ii < n; ii++) {
					tij += b.get(i, ii) * c.get(ii, j);
				}
				// contribution
				for (int jj = 0; jj < m; jj++) {
					k.add(i, jj, alpha * tij * b.get(jj, j));
				}
			}
		}
	}

	/**
	 * Computes k <- k + alpha a^T c b.
	 * 
	 * @param a
	 *            matrix of size m x n
	 * @param c
	 *            matrix of size m x m
	 * @param b
	 *            matrix of size m x p
	 * @param alpha
	 *            scaling factor
	 * @param k
	 *            matrix of size n x p
	 */
	public static void atCB(IMatrixReadOnly a, IMatrixReadOnly b, IMatrixReadOnly c, double alpha,
			IMatrix k) {
		int m = a.getRowCount();
		int n = a.getColumnCount();
		int p = b.getColumnCount();
		assert m == c.getRowCount() && m == c.getColumnCount() && m == b.getRowCount();
		assert n == k.getRowCount() && p == k.getColumnCount();

		// rows of a^T
		for (int i = 0; i < n; i++) {
			// columns of c
			for (int j = 0; j < m; j++) {
				// s is the ij-th entry of a^T c and computes as the
				// dot product of i-th row of a^T with j-th column of c
				double tij = 0;
				for (int ii = 0; ii < m; ii++) {
					tij += a.get(ii, i) * c.get(ii, j);
				}
				// contribution
				for (int jj = 0; jj < p; jj++) {
					k.add(i, jj, alpha * tij * b.get(j, jj));
				}
			}
		}
	}

	/**
	 * Computes k <- k + alpha g^T h^(-1) g.
	 * 
	 * @param g
	 * @param h
	 * @param alpha
	 * @param k
	 */
	public static void gtHiG(IMatrixReadOnly g, Array1DMatrix h, double alpha, IMatrix k) {
		IMatrix tmp = (IMatrix) g.clone();
		backSolve(h, tmp);
		BLAM.multiply(alpha, BLAM.TRANSPOSE, g, BLAM.NO_TRANSPOSE, tmp, 1.0, k);
	}

	/**
	 * compute trace of tensor in vector form
	 * 
	 * @param sigma
	 *            (either sigma or epsilon)
	 * @return trace of second order tensor in Voigt notation
	 */
	public static double trace(IVector sigma) {
		if (sigma.getSize() == 3) {
			return sigma.get(0) + sigma.get(1);
		} else if (sigma.getSize() == 6) {
			return sigma.get(0) + sigma.get(1) + sigma.get(2);
		} else {
			throw new Error("invalid tensor in Voigt notation");
		}
	}

	/**
	 * 
	 * @param m
	 *            square matrix
	 * @return trace of m
	 */
	public static double trace(IMatrix m) {
		int n = m.getRowCount();
		assert (n == m.getColumnCount());
		double tr = 0;
		for (int i = 0; i < n; i++) {
			tr += m.get(i, i);
		}
		return tr;
	}

	/**
	 * perform spectral decomposition to find princlple stress/strain in Voigt
	 * notation
	 * 
	 * @param dim
	 * @param s
	 * @return if dim == 2, return [o1, o2] (o1 > o2)<br>
	 *         if dim == 3, return [o1, o2, o3] (o1 > o2 > o3)
	 */
	public static double[] spectralDecompose(int dim, IVector s) {
		if (dim == 2) {
			double oxx = s.get(0);
			double oyy = s.get(1);
			double oxy = s.get(2);
			double t1 = (oxx + oyy) / 2;
			double t2 = Math.sqrt(Math.pow((oxx - oyy) / 2, 2) + Math.pow(oxy, 2));
			double o1 = t1 + t2;
			double o2 = t1 - t2;
			return new double[] { o1, o2 };
		} else if (dim == 3) {
			double oxx = s.get(0);
			double oyy = s.get(1);
			double ozz = s.get(2);
			double oxy = s.get(3);
			double oyz = s.get(4);
			double ozx = s.get(5);
			double I1 = oxx + oyy + ozz;
			double I2 = oxx * oyy + oyy * ozz + ozz * oxx - oxy * oxy - oyz * oyz - ozx * ozx;
			double I3 = oxx * oyy * ozz - oxx * oyz * oyz - oyy * ozx * ozx - ozz * oxy * oxy + 2
					* oxy * oyz * ozx;
			double tmp = (2 * Math.pow(I1, 3) - 9 * I1 * I2 + 27 * I3)
					/ (2 * Math.pow(I1 * I1 - 3 * I2, 1.5));
			if (Math.abs(tmp - 1.0) < MathUtilities.tol6) {
				tmp = 1.0;
			} else if (Math.abs(tmp + 1.0) < MathUtilities.tol6) {
				tmp = -1.0;
			} else if ((tmp + 1.0 < 0) || (tmp - 1.0 > 0)) {
				throw new Error("phi is not correct computed, tmp = " + tmp);
			}
			double phi = 1.0 / 3 * Math.acos(tmp);
			double tmp1 = I1 / 3;
			double tmp2 = 2.0 / 3 * Math.sqrt(I1 * I1 - 3 * I2);
			double o1 = tmp1 + tmp2 * Math.cos(phi);
			double o2 = tmp1 + tmp2 * Math.cos(phi + 2 * Math.PI / 3);
			double o3 = tmp1 + tmp2 * Math.cos(phi + 4 * Math.PI / 3);
			// return new double[] { o1, o2, o3 };
			if (o1 >= o2) {
				if (o2 >= o3) {
					return new double[] { o1, o2, o3 };
				} else {
					if (o1 >= o3) {
						return new double[] { o1, o3, o2 };
					} else {
						return new double[] { o3, o1, o2 };
					}
				}
			} else {
				if (o1 >= o3) {
					return new double[] { o2, o1, o3 };
				} else {
					if (o2 >= o3) {
						return new double[] { o2, o3, o1 };
					} else {
						return new double[] { o3, o2, o1 };
					}
				}
			}
		} else {
			throw new Error("invalid dimension");
		}
	}

	/**
	 * perform spectral decomposition to find princlple stress in Voigt notation
	 * 
	 * @param dim
	 * @param s
	 * @param ps
	 *            vector of principal stress, must be initialized before using
	 * @param pd
	 *            matrix of principle direction, must be initialized before
	 *            using
	 */
	public static void decomposePrincipalStress(int dim, IVector s, IVector ps, IMatrix pd) {
		PrincipalStressStrainDecomposer.getAInfo().setSize(dim);
		PrincipalStressStrainDecomposer.initialize();
		IMatrix A = PrincipalStressStrainDecomposer.getA();
		if (dim == 2) {
			A.set(0, 0, s.get(0));
			A.set(0, 1, s.get(2));
			A.set(1, 0, s.get(2));
			A.set(1, 1, s.get(1));
		} else if (dim == 3) {
			A.set(0, 0, s.get(0));
			A.set(1, 1, s.get(1));
			A.set(2, 2, s.get(2));
			A.set(0, 1, s.get(3));
			A.set(0, 2, s.get(5));
			A.set(1, 0, s.get(3));
			A.set(1, 2, s.get(4));
			A.set(2, 0, s.get(5));
			A.set(2, 1, s.get(4));
		}
		try {
			PrincipalStressStrainDecomposer.solve(null);
		} catch (SolveFailedException e) {
			throw new Error("Decompose failed");
		}
		IMatrix D = PrincipalStressStrainDecomposer.getD();
		for (int i = 0; i < dim; i++) {
			ps.set(i, D.get(i, i));
		}
		if (pd != null) {
			BLAM.copy(A, pd);
		}
	}

	/**
	 * find all eigenvalues of matrix A
	 * 
	 * @param A
	 * @return
	 */
	public static JArpackSBMGESolver createEigenvalueSolver(IMatrix A, IMatrix B) {
		JArpackSBMGESolver geSolver = new JArpackSBMGESolver();

		assert A.getColumnCount() == A.getRowCount();

		int n = A.getRowCount();

		// SymmetricBandMatrixInfo geAInfo = (SymmetricBandMatrixInfo)
		// geSolver.getAInfo();
		// SymmetricBandMatrixInfo geBInfo = (SymmetricBandMatrixInfo)
		// geSolver.getBInfo();

		// geAInfo.setSize(n);
		// geAInfo.setBandwidth(2 * n - 1);
		// geBInfo.setSize(n);
		// geBInfo.setBandwidth(2 * n - 1);

		geSolver.getAInfo().setSize(n);
		geSolver.getBInfo().setSize(n);

		int nev = 1;
		geSolver.setNev(nev);
		geSolver.setSigma(1);
		geSolver.setMode(JArpackSBMGESolver.MODE_BUCKLING);
		geSolver.initialize();

		BLAM.copy(A, geSolver.getA());
		BLAM.copy(B, geSolver.getB());

		try {
			geSolver.solve();
		} catch (SolveFailedException e) {
			throw new Error("solve failed");
		}

		return geSolver;
	}

	/**
	 * solve the quadratic equation a x^2 + bx + c = 0
	 * 
	 * @param a
	 * @param b
	 * @param c
	 * @return
	 */
	public static double[] solveQuadratic(double a, double b, double c) {
		if (a == 0) {
			if (b == 0) {
				return null;
			} else {
				return new double[] { -c / b };
			}
		} else {
			double D = b * b - 4 * a * c;
			if (D < 0) {
				return null;
			} else if (D == 0) {
				return new double[] { -b / (2 * a) };
			} else {
				return new double[] { (-b - Math.sqrt(D)) / (2 * a), (-b + Math.sqrt(D)) / (2 * a) };
			}
		}
	}
}
